{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Import the necessary packages"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import math\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "np.set_printoptions(threshold=np.nan)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Vehicle Parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "SPEED = 1.4\n",
    "LENGTH = 1.5\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "vehicle_points = np.array([[-1.1, 2],[0, 2],[1.1, 2],[-1.1, 0],[0, 0],[1.1,0],[-1.1, -2],[0, -2],[1.1, -2]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "max_angle = 0.785398\n",
    "min_angle = -0.785398"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Stat Model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "class state:\n",
    "\n",
    "    def __init__(self, x, y, theta, g,f, steer):\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "        self.theta = theta\n",
    "        self.g = g\n",
    "        self.f = f\n",
    "        self.steer = steer\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### State Transition"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def transition(v_state, delta, dt, goal):\n",
    "   \n",
    "    # ---Begin bicycle model---\n",
    "    #delta_rad = deg_to_rad(delta)\n",
    "    omega = SPEED/LENGTH * math.tan(delta)\n",
    "    \n",
    "    \n",
    "    d_x = SPEED * math.cos(v_state.theta) * dt\n",
    "    d_y = SPEED * math.sin(v_state.theta) * dt\n",
    "    \n",
    "    next_x = v_state.x + d_x\n",
    "    next_y = v_state.y + d_y\n",
    "    next_theta = normalize(v_state.theta + (omega*dt)) \n",
    "\n",
    "    # ---End bicycle model-----\n",
    "    next_g = v_state.g + np.sqrt( d_x * d_x +  d_y * d_y )\n",
    "    next_f = next_g+heuristic(next_x, next_y, goal)\n",
    "    next_steer = delta\n",
    "\n",
    "    # Create a new State object with all of the \"next\" values.\n",
    "    new_state = state(next_x, next_y, next_theta, next_g, next_f, next_steer)\n",
    "\n",
    "    return new_state"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Grid Resolution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "grid_x_m = 20\n",
    "grid_y_m = 20\n",
    "\n",
    "coll_cell_side = 1\n",
    "grid_coll_x = np.int( np.ceil(grid_x_m/coll_cell_side) )\n",
    "grid_coll_y = np.int( np.ceil(grid_y_m/coll_cell_side) )\n",
    "\n",
    "grid = np.zeros((grid_coll_x,grid_coll_y), np.int)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "cell_side = 1.0\n",
    "x_offset = (grid.shape[1]*coll_cell_side)/2\n",
    "y_offset = 0.0\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### A Class to store the state and th index"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "class link:\n",
    "    def __init__(self, inp_state, inp_index):\n",
    "        self.state = inp_state\n",
    "        self.index = inp_index\n",
    "        "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Start And Goal"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "class start:\n",
    "\n",
    "    def __init__(self, x, y, theta):\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "        self.theta = theta\n",
    "        \n",
    "class goal:\n",
    "\n",
    "    def __init__(self, x, y):\n",
    "        self.x = x\n",
    "        self.y = y"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Helper Functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "def deg_to_rad(delta):\n",
    "    return delta * math.pi/180"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "def rad_to_deg(delta):\n",
    "    return delta * 180 / math.pi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "def heuristic(x,y,goal):\n",
    "    distance  = math.sqrt( ((x-goal.x)**2) + ((y-goal.y)**2) )\n",
    "    return distance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "def normalize(theta):\n",
    "    if theta < 0:    \n",
    "        theta = theta + 2.0 * math.pi\n",
    "        return theta\n",
    "    if theta > 2*np.pi:    \n",
    "        theta = theta - 2.0 * math.pi\n",
    "        return theta\n",
    "    else:\n",
    "        return theta "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "def rotation(w,t):\n",
    "    y = np.array([[math.cos(t), math.sin(t)], [-math.sin(t), math.cos(t)]])\n",
    "    return np.matmul(w, y)\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "def transformation(w, x, y,t):\n",
    "    p = np.zeros_like(w)\n",
    "    for i in range(p.shape[0]):\n",
    "        p[i][0:2] = w[i][0:2]+[x,y]\n",
    "        \n",
    "    return rotation(p,(t-np.pi/2))\n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Fill Obstacles in the Grid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "grid[9][0:18] = 1\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]]\n"
     ]
    }
   ],
   "source": [
    "print(grid)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_grid_coordinates(x, y):\n",
    "    x = x+x_offset\n",
    "    y = y+y_offset\n",
    "    cell_x = math.floor(x/cell_side)\n",
    "    cell_y = math.floor(y/cell_side)\n",
    "    #grid[cell_y, cell_x] = 2\n",
    "    return cell_x, cell_y\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "def check_occupancy(x, y):\n",
    "    x = x+x_offset\n",
    "    y = y+y_offset\n",
    "    cell_x = math.floor(x/coll_cell_side)\n",
    "    cell_y = math.floor(y/coll_cell_side)\n",
    "    #grid[cell_y, cell_x] = 2\n",
    "    return grid[cell_y, cell_x]\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "def delta_t(delta, n, off):\n",
    "    #delta = deg_to_rad(delta)#for convenient debugging\n",
    "    #off = deg_to_rad(off)#for convenient debugging\n",
    "    delta_list = []\n",
    "    delta_list.append(delta)\n",
    "    delta_calc = delta\n",
    "    for i in range(0 ,n):\n",
    "        delta_calc += off   \n",
    "        if delta_calc < max_angle:\n",
    "            delta_list.append(delta_calc)\n",
    "        \n",
    "    delta_calc = delta\n",
    "    for i in range(0 ,n):\n",
    "        delta_calc -= off\n",
    "        if min_angle < delta_calc:\n",
    "            delta_list.append(delta_calc)\n",
    "        \n",
    "    return delta_list"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "def checkLimits( x,y ):\n",
    "    \n",
    "    result = True\n",
    "    \n",
    "    if  ( x < -grid_x_m/2 ) | \\\n",
    "        ( y < 0 ) | \\\n",
    "        ( x > grid_x_m/2 ) | \\\n",
    "        ( y > grid_y_m ):\n",
    "        \n",
    "        result = False\n",
    "    \n",
    "    return result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "def check_closed(x, y, theta):\n",
    "    x = x + x_state_offset\n",
    "    y = y + y_state_offset\n",
    "    \n",
    "    cell_x = math.floor(x/state_cell_side)\n",
    "    cell_y = math.floor(y/state_cell_side)\n",
    "    \n",
    "    cell_theta = math.floor( theta * NUM_THETA_CELLS / (2.0 * np.pi) )\n",
    "    return closed[cell_theta][cell_y][cell_x]\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "def set_closed(x, y, theta):\n",
    "    x = x + x_state_offset\n",
    "    y = y + y_state_offset\n",
    "    \n",
    "    cell_x = math.floor(x/state_cell_side)\n",
    "    cell_y = math.floor(y/state_cell_side)\n",
    "    \n",
    "    cell_theta = math.floor( theta * NUM_THETA_CELLS / (2.0 * np.pi) )\n",
    "    closed[cell_theta][cell_y][cell_x] = 1\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "current_state = state(0.0, 2.1, np.pi/2, 0.0, 0.0, 0.0)\n",
    "new_points = np.copy(vehicle_points)\n",
    "#print(new_points)\n",
    "Goal = goal(0.0, 18.0)\n",
    "\n",
    "\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "state_cell_side = 1\n",
    "grid_state_x = np.int( np.ceil(grid_x_m/state_cell_side) )\n",
    "grid_state_y = np.int( np.ceil(grid_y_m/state_cell_side) )\n",
    "\n",
    "NUM_THETA_CELLS = 360\n",
    "closed = [[[0 for x in range(grid_state_x)] for y in range(grid_state_y)] for cell in range(NUM_THETA_CELLS)]\n",
    "\n",
    "x_state_offset = grid_x_m/2.0\n",
    "y_state_offset = 0.0\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [],
   "source": [
    "#print(current_state.x, current_state.y, current_state.theta)\n",
    "\n",
    "path_state = current_state\n",
    "open_list = []\n",
    "path_link = link( path_state, 0 )\n",
    "open_list.append( path_link )\n",
    "\n",
    "trace_list = []\n",
    "trace_link = link( path_state, -1 )\n",
    "trace_list.append( trace_link )\n",
    "\n",
    "\n",
    "path_x = []\n",
    "path_y = []\n",
    "\n",
    "path_x.append( current_state.x )\n",
    "path_y.append( current_state.y )\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "goal\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x28ad39c8518>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "goal_reached = 0\n",
    "\n",
    "for count in range(2):\n",
    "    while ( len(open_list) > 0 ) & (goal_reached == 0):\n",
    "\n",
    "        open_list.sort( key=lambda ste:ste.state.f )\n",
    "\n",
    "        path_link = open_list.pop(0)\n",
    "        path_state = path_link.state\n",
    "        path_index = path_link.index\n",
    "\n",
    "       # print( \"\" )\n",
    "        #print( \"Best path\")\n",
    "        #print( path_state.x, path_state.y, path_state.theta, path_state.f )\n",
    "        path_x.append( path_state.x )\n",
    "        path_y.append( path_state.y )\n",
    "\n",
    "\n",
    "        #exit = 0\n",
    "\n",
    "        #while exit == 0:\n",
    "\n",
    "        delta_angle = delta_t( path_state.steer, 5, deg_to_rad(5.0) )\n",
    "        delta_angle = [ 30 * np.pi / 180, 20 * np.pi / 180, 0, -20 * np.pi / 180, -30 * np.pi / 180 ]\n",
    "\n",
    "        #print( delta_angle )\n",
    "\n",
    "        state_list = []\n",
    "\n",
    "        for i in range(len(delta_angle)):\n",
    "            \n",
    "            new_state = transition( path_state, delta_angle[i], 0.1, Goal)\n",
    "            \n",
    "            #print( new_state.x, new_state.y, new_state.theta )\n",
    "            if check_closed( new_state.x, new_state.y, new_state.theta ) == 0:\n",
    "                if checkLimits( new_state.x , new_state.y ) == True:\n",
    "                    state_list.append(new_state)\n",
    "                    set_closed( new_state.x, new_state.y, new_state.theta )\n",
    "                    \n",
    "                    #print( new_state.x, new_state.y, new_state.steer )\n",
    "\n",
    "        collision_free = []\n",
    "\n",
    "        for i in range(len(state_list)):\n",
    "\n",
    "            check = 0\n",
    "            #new_points = transformation(vehicle_points, state_list[i].x, state_list[i].y, state_list[i].theta)\n",
    "            new_points = [ [ state_list[i].x, state_list[i].y ] ]\n",
    "            \n",
    "            for point in new_points :\n",
    "                if checkLimits(point[0],point[1]) == True:\n",
    "\n",
    "                    get_x, get_y = get_grid_coordinates(point[0], point[1])\n",
    "                    \n",
    "                    check = check_occupancy(point[0], point[1])\n",
    "                   \n",
    "                else :\n",
    "                    check =1\n",
    "\n",
    "                if check == 1:\n",
    "                \n",
    "                   break\n",
    "\n",
    "            if check == 0:\n",
    "                collision_free.append(state_list[i])\n",
    "                \n",
    "\n",
    "        if( collision_free ):\n",
    "\n",
    "            for nstate in collision_free:\n",
    "                \n",
    "                #print( nstate.x, nstate.y, nstate.theta )\n",
    "                new_link = link( nstate, len(trace_list) )\n",
    "                open_list.append( new_link )\n",
    "                \n",
    "                trace_link = link( nstate, path_index )\n",
    "                trace_list.append( trace_link )\n",
    "                          \n",
    "                \n",
    "                if np.sqrt((nstate.x - Goal.x)**2 + (nstate.y - Goal.y)**2) < 0.5:\n",
    "                    goal_reached = 1\n",
    "                    path_x.append( nstate.x )\n",
    "                    path_y.append( nstate.y )\n",
    "                    print( \"goal\")\n",
    "                    \n",
    "\n",
    "\n",
    "            #path_state = collision_free[0]\n",
    "\n",
    "            #if checkLimits( path_state ) == True:\n",
    "            #    path_x.append( path_state.x )\n",
    "            #    path_y.append( path_state.y )\n",
    "\n",
    "                #print( path_state.x, path_state.y )\n",
    "\n",
    "\n",
    "    #print( \"\" )\n",
    "    #print( \"Ostates\")\n",
    "    #for ostate in open_list:\n",
    "\n",
    "       # print( ostate.x, ostate.y, ostate.theta, ostate.f )\n",
    "\n",
    "plt.scatter( path_x, path_y )\n",
    "plt.axis('equal')\n",
    "plt.show()\n",
    "        \n",
    "    \n",
    "#print(current_state.x, current_state.y, current_state.theta)\n",
    "#print(new_points)\n",
    "#grid_v = np.copy(grid)\n",
    "\n",
    "#    if check == 1:\n",
    "#        break\n",
    "\n",
    "#if check == 0:\n",
    "#    print(grid_v)\n",
    "\n",
    "#print(grid_v)\n",
    "\n",
    "#plt.scatter(px,py)\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "#plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAXQAAAD8CAYAAABn919SAAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAFOlJREFUeJzt3XGQnHddx/HPJ8kxXrCYMD2wvTYGGMgMIdDDk4kwIhRpopD2dBTptE7VjnEYQagYbIcMCUzVDqGCMzo40cZ0prVQMRzBUdJaxI5OU930Ui5HiWWwhFwquU576pgbe02//pG95nK9ffbZ3WefZ/d53q+ZTO72eZL97qT3mW+/z+95fo4IAQD634qiCwAAZINAB4CSINABoCQIdAAoCQIdAEqCQAeAkiDQAaAkCHQAKAkCHQBKYlWeb3bxxRfH+vXr83xLAOh7R44ceSoihpqdl2ugr1+/XrVaLc+3BIC+Z/t7ac5j5AIAJUGgA0BJEOgAUBJNA932PtunbR9b9NoVtg/bPmq7Zvst3S0TANBMmg59v6StS177tKRPRsQVkj5R/x4AUKCmq1wi4kHb65e+LOll9a9/RNKpbMtCPxmfmNaeQ8d1anZOl64Z1I4tGzQ2Mlx0WUDltLts8SOSDtn+jM51+W/NriT0k/GJad1yYFJz82clSdOzc7rlwKQkEepAztq9KPoBSTdFxOWSbpJ0R6MTbW+vz9lrMzMzbb4detWeQ8dfCPMFc/NntfvgVEEVAdXVbqDfIOlA/eu/ltTwomhE7I2I0YgYHRpqeqMT+syp2bllX5+dm9fO8cmcqwGqrd1APyXpp+tfXynp8WzKQb+5dM1gw2N3HT5BqAM5SrNs8R5JD0naYPuk7Rsl/Yak220/KukPJG3vbpnoVTu2bEg8fvfhExqfmM6pGqDa0qxyubbBoR/PuBb0obGRYX3yq1N65sz8ssdD0kfvffSFcwF0D3eKomO7tm2UE46fjdBNXzzK+AXoMgIdHRsbGdZ1m9clnhNipg50G4GOTNw6tknXb16X2KlL50J95FP3MVcHuoBAR2ZuHdukz/7yFVrp5Fh/5sw8IxigCwh0ZGpsZFi3v+9NTTv1hREM3TqQHQIdmUszU19Atw5kh0BHV6SdqUt060BWCHR0zcJMfc3gQKrz6daBzhDo6KqxkWEd3XUV3TqQAwIduaBbB7qPQEdu6NaB7iLQkTu6daA7CHQUop1unSc3AskIdBSqlW49JHZCAhIQ6ChcK906OyEBjRHo6Blpu3We2ggsL82ORftsn7Z9bMnrH7J93PaU7U93r0RUyeJuPQnzdODF0nTo+yVtXfyC7XdKukbSGyNio6TPZF8aquzWsU1au7pxp848HXixpoEeEQ9KenrJyx+QdFtE/F/9nNNdqA0V12wnJObpwIXanaG/TtJP2X7Y9j/Z/oksiwKkdE9tZPQCnNduoK+StFbSZkk7JN1rL7+rge3ttmu2azMzM22+Hapq4amNjTB6Ac5rN9BPSjoQ5/yrpOclXbzciRGxNyJGI2J0aGio3TpRYc3m6YxegHPaDfRxSVdKku3XSXqJpKeyKgpYqtk8ndELkG7Z4j2SHpK0wfZJ2zdK2ifp1fWljF+QdENERHdLRZU1m6czegHSrXK5NiIuiYiBiLgsIu6IiGcj4vqIeENEvDkivp5Hsag2Ri9AMu4URV9pNnrhcbuoMgIdfSXNUkYet4uqItDRd5qNXqTzm2MQ6qgSAh19qdnoZQEjGFQJgY6+tDB6SRPqjGBQFQQ6+larm2PQraPsCHT0tbSP213wzJl5feSLRwl2lJLzvB9odHQ0arVabu+Hatk5Pqm7D59Qq/9FD68Z1I4tGzQ2MtyVuoBO2T4SEaNNzyPQUSbjE9PafXBKs3Pzbf35tasHtGvbRsIdPSVtoDNyQam0sj/pchjJoJ8R6CilVi6YLoeVMehHjFxQep2OYVZYej6YtaM4jFyAuk7HMM/Xe57p2Tm6dvQ0Ah2VsTCGGV4zKElthXuIZ6+jdzFyQaW1O45ZMzigo7uu6lJVwIUYuQApLIxjnrjtPS2NZGbn5lkJg56TZseifbZP13cnWnrsd22H7WX3EwX6SasrY1gJg16TpkPfL2nr0hdtXy7p3ZJOZFwTUJiFjv1zi2btSXhML3pJmi3oHpT09DKHPivpY1LLd1oDPW9sZFj/cvOVeuK29zR99rrEg7/QG9qaodu+WtJ0RDyacT1Az9m1baMGB1Y2Pe+ZM/O65cAkoY7CtBzotldL+rikT6Q8f7vtmu3azMxMq28HFG5sZFh/+AubUs3W5+bPavfBqRyqAl6snQ79NZJeJelR209IukzSI7Z/dLmTI2JvRIxGxOjQ0FD7lQIFauUxvbNz88zUUYhVrf6BiJiU9IqF7+uhPhoRT2VYF9CTbh3bJElNH9N71+ETF5wP5CHNssV7JD0kaYPtk7Zv7H5ZQO9Ku7yRC6XIG3eKAh0Y+dR9euZM8l2mlnTd5nV062gbd4oCOdi1bWPTu0vZzxR5IdCBDoyNDOu6FvYz5c5SdBOBDnTo1rFNqZ8DQ7eObiLQgQzwHBj0AgIdyEirG2nQrSNrBDqQMbp1FIVAB7qAbh1FINCBLqJbR54IdKDL2u3WCXW0ikAHctJqt84IBq0i0IEctdqtM4JBKwh0oACtdOtcMEVaBDpQkFaesS7RraO5lp+HDiBbaZ+xLp3v1hf/OWABHTrQA1q9YHr34ROMX/AiBDrQI1q5YBqSPnrvo4Q6LkCgAz0mbbd+NoKZOi6QZgu6fbZP2z626LU9tr9t+5u2v2x7TXfLBKol7QVTbkLCYmk69P2Sti557X5Jb4iIN0r6d0m3ZFwXAKV/1jozdUgpAj0iHpT09JLX7ouI5+rfHpZ0WRdqA6DzI5iVbhzrIWn3wan8ikJPymKG/uuS/r7RQdvbbdds12ZmZjJ4O6B6xkaGdfv73pTYqc/OzTN6qbiOAt32xyU9J+nuRudExN6IGI2I0aGhoU7eDqi0NPuXMnqptrYD3fYNkt4r6bqIaHY/BIAMLMzUG2H0Um1tBbrtrZJ+T9LVEXEm25IAJLl1bJPWrm68pJHRS3WlWbZ4j6SHJG2wfdL2jZL+RNJFku63fdT2n3W5TgCL7Nq2MXGezuilmpo+yyUirl3m5Tu6UAuAlMZGhlX73tMvPNdlqZC059BxjY0M51sYCsWdokCfajZ6mZ6dy7Ea9AICHehju7ZtbHgszQYaKBcCHehjSSOVkJijVwyBDvS54TWDDY+xhLFaCHSgz+3YsqHhsdm5+RwrQdEIdKDPNVvJwtilOgh0oASSVrvsOXQ8x0pQJAIdKIGk1S4sX6wOAh0ogbGRYa1osE4x6bG7KBcCHSiJ5xs8Iu8sz86rDAIdKIlGfTj9eXUQ6EBJNOrD6c+rg0AHgJIg0IGSaHRRtNHrKB8CHSiJRhdFG72O8iHQgZJotDyRZYvVkWbHon22T9s+tui1l9u+3/bj9d/XdrdMAM00Wp7IssXqSNOh75e0dclrN0t6ICJeK+mB+vcACsSyRTQN9Ih4UNLTS16+RtKd9a/vlDSWcV0AWsSyRbQ7Q39lRDwpSfXfX5FdSQCAdnT9oqjt7bZrtmszMzPdfjsAS7BssTraDfQf2L5Ekuq/n250YkTsjYjRiBgdGhpq8+0AJEl65jnLFquj3UA/KOmG+tc3SPpKNuUAaEfSM8+TtqhDuaRZtniPpIckbbB90vaNkm6T9G7bj0t6d/17AAVJeuZ50hZ1KJdVzU6IiGsbHHpXxrUAaJPVeDVLsy3qUB7cKQqUAGNySAQ6AJQGgQ6UAE9ahESgA6XAkxYhEehAKfCkRUgEOlAKPGkREoEOlEJSJ550FynKhUAHSiCpE0+6ixTlQqADJZB0e3/SXaQoFwIdKIGk2/u5MFodBDpQAkm393NhtDoIdKAkWLoIAh0oCZYugkAHSoIOHQQ6UBJ06CDQgZJo1IfTn1dHR4Fu+ybbU7aP2b7H9g9lVRiA1jTqw+nPq6PtQLc9LOm3JY1GxBskrZT0/qwKAwC0ptORyypJg7ZXSVot6VTnJQHIEs9Er462Az0ipiV9RtIJSU9K+q+IuC+rwgCkl/QALp6JXh2djFzWSrpG0qskXSrppbavX+a87bZrtmszMzPtVwqgoaQHcCU95wXl0snI5Wck/UdEzETEvKQDkt669KSI2BsRoxExOjQ01MHbAWgk6QFcSc95Qbl0EugnJG22vdq2Jb1L0mPZlAUgraRxywonP+cF5dLJDP1hSV+S9IikyfrftTejugCklDRuYX5eLas6+cMRsUvSroxqAdCGpHEL8/Nq4U5RoI81216O+Xm1EOhAH9t9cCrxOPPzaiHQgT41PjGt2bn5hscZt1QPgQ70qaSLoRbjlioi0IE+lXQx9LrN6xi3VBCBDvSpRo9oWWHp1rFNudaC3kCgA31ofGK64WNxWXteXQQ60IeSVrdwMbS6CHSgDyWtbuFiaHUR6EDJcDG0ugh0oETYzKLaCHSgz+wcn2x4jAui1UagA31kfGJadx8+0fA4F0SrjUAH+sieQ8cbLleUuCBadQQ60EeS7g5du3qAC6IVR6ADfSTpmueubRtzqwO9iUAH+sTO8cnEcQvdOToKdNtrbH/J9rdtP2b7J7MqDMB5XAxFGh1tQSfpjyV9LSJ+0fZLJK3OoCYAS+w+OMXFUDTVdqDbfpmkt0v6VUmKiGclPZtNWQAW7ByfTLzVn4uhWNDJyOXVkmYk/aXtCdt/YfulS0+yvd12zXZtZmamg7cDqmfn+KTuShi1WFwMxXmdBPoqSW+W9PmIGJH0v5JuXnpSROyNiNGIGB0aGurg7YBqaTY3l9jIAhfqJNBPSjoZEQ/Xv/+SzgU8gAw0m5uvXT3ARha4QNuBHhH/Ken7theuxrxL0rcyqQqouGZzc0YtWE6nq1w+JOnu+gqX70r6tc5LAqprfGJan/zqlJ450zjMJUYtWF5HgR4RRyWNZlQLUGk7xyd19+ETiWMWSbp+8zpGLVhWpx06gA6l7col5uZIRqADBUrblUvMzdEcgQ4UoJWuXDoX5szN0QyBDuSsla5cktYMDmj31RsJczRFoAM5aLUjl8535czMkRaBDtQtDV1LCkkrbZ2NeOH34TWD2rFlw7Id8/jEtPYcOq7p2bkXzl/h1vf6pCtHOxyR366yo6OjUavVcns/oJF2OublLA39TtGVYzm2j0RE0yXidOjoWc065sWdb7vHOrXw12QR5nTl6BSBjp6xeFyxXOguDc/Fx9s91gvoypEVAh09YenKj14L3W6hK0eWCHQUolk3XnYEObqBQEfu+qUbz2r2nmZ1DJAFAh25yWplSTcshHez0M1iaSPQLQQ6ctHq3ZHLyWqVSyehOzYyTEijZxHo6KpOu3I6XSA9Ah1d025XzgVDoD0dB7rtlZJqkqYj4r2dl4QyaLZb/XLoxoHOZNGhf1jSY5JelsHfhRJIs1v9ArpxIDsdBbrtyyS9R9LvS/qdTCpC32u2W73E3ZFAN3TaoX9O0sckXdToBNvbJW2XpHXr1nX4duhlCxdAk3arl+jKgW5pO9Btv1fS6Yg4Yvsdjc6LiL2S9krnnrbY7vuht41PTOuWA5Oamz+beB4bHAPds6KDP/s2SVfbfkLSFyRdafuuTKpC39l9cIowBwrWdqBHxC0RcVlErJf0fklfj4jrM6sMfWPn+GTTMQu71QPd10mHDqRa0TI4sJLd6oEcZHJjUUR8Q9I3svi70F+arWjhAiiQH+4URdvGJ6YTRy1rVw9o4hNX5VgRUG2MXNC23QenGh6zxJgFyBmBjrYldefXbV7HmAXIGYGOtuwcn0w8zooWIH8EOlrWbGXL2tUDOVYDYAGBjpbtOXQ8cWULs3OgGAQ6WjY9O9fw2NrVA8zOgYIQ6GiZE47RnQPFIdDRkvGJ6cRxC905UBwCHS1JWns+vGYwx0oALEWgoyVJa893bNmQYyUAliLQkRnGLUCxCHRkYkXSlVIAuSDQkdr4xHTDY8+zFxVQOAIdqe05dLzhMS6IAsUj0JFa0g1FXBAFitd2oNu+3PY/2n7M9pTtD2dZGHrPSi8/KF9hLogCvaCTDS6ek/TRiHjE9kWSjti+PyK+lVFt6DFnY/lBOfNzoDd0skn0kxHxSP3r/5H0mCTatBJrNCdnfg70hkxm6LbXSxqR9HAWfx96044tGzQ4sPKC1wYHVjI/B3pEx3uK2v5hSX8j6SMR8d/LHN8uabskrVu3rtO3Q4EW5uR7Dh3Xqdk5XbpmUDu2bGB+DvQIR4O5aKo/bA9I+ltJhyLij5qdPzo6GrVare33A4Aqsn0kIkabndfJKhdLukPSY2nCHADQXZ3M0N8m6VckXWn7aP3Xz2VUFwCgRW3P0CPin5W81wEAIEfcKQoAJUGgA0BJdLTKpeU3s2ckfS+3N+y+iyU9VXQRBeGzV1eVP39Rn/3HImKo2Um5BnrZ2K6lWUpURnz2an52qdqfv9c/OyMXACgJAh0ASoJA78zeogsoEJ+9uqr8+Xv6szNDB4CSoEMHgJIg0Dtke7ft6So9/sD2VtvHbX/H9s1F15Mn20/Ynqz/W5f6SXO299k+bfvYotdebvt+24/Xf19bZI3d1ODz9/TPO4Gejc9GxBX1X39XdDHdZHulpD+V9LOSXi/pWtuvL7aq3L2z/m/ds8vXMrJf0tYlr90s6YGIeK2kB+rfl9V+vfjzSz38806go1VvkfSdiPhuRDwr6QuSrim4JnRBRDwo6eklL18j6c7613dKGsu1qBw1+Pw9jUDPxgdtf7P+v2il/V/QumFJ31/0/UlVa+vBkHSf7SP1zVuq5pUR8aR0bhtKSa8ouJ4i9OzPO4Gegu1/sH1smV/XSPq8pNdIukLSk5JuL7TY7lvuCZtVWir1toh4s86NnH7L9tuLLgi56umf9463oKuCiPiZNOfZ/nOd28GpzE5KunzR95dJOlVQLbmLiFP130/b/rLOjaAeLLaqXP3A9iUR8aTtSySdLrqgPEXEDxa+7sWfdzr0DtX/o17w85KONTq3JP5N0mttv8r2SyS9X9LBgmvKhe2X2r5o4WtJV6n8/95LHZR0Q/3rGyR9pcBactfrP+906J37tO0rdG7s8ISk3yy2nO6KiOdsf1DSIUkrJe2LiKmCy8rLKyV9+dzui1ol6a8i4mvFltQ9tu+R9A5JF9s+KWmXpNsk3Wv7RkknJP1ScRV2V4PP/45e/nnnTlEAKAlGLgBQEgQ6AJQEgQ4AJUGgA0BJEOgAUBIEOgCUBIEOACVBoANASfw/oCIu5AKh298AAAAASUVORK5CYII=\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x28ad392f940>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "if goal_reached == 1 :\n",
    "        fpath_x = []\n",
    "        fpath_y = []\n",
    "        \n",
    "        curr_trace = trace_list[-1]\n",
    "        \n",
    "        while( curr_trace.index != -1 ):\n",
    "            fpath_x.append( curr_trace.state.x )\n",
    "            fpath_y.append( curr_trace.state.y )\n",
    "            \n",
    "            curr_trace = trace_list[ curr_trace.index ]\n",
    "            \n",
    "plt.scatter( fpath_x, fpath_y )\n",
    "plt.axis( 'equal' )\n",
    "plt.show()\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x28ad5104c88>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "steer = 50 * np.pi / 180\n",
    "cstate = state( 0, 0, np.pi/2, 0, 0, steer )\n",
    "px = [] \n",
    "py = []\n",
    "\n",
    "px.append( cstate.x )\n",
    "py.append( cstate.y )\n",
    "\n",
    "for i in range(150):\n",
    "    cstate = transition( cstate, steer, 0.1, Goal)\n",
    "    px.append( cstate.x )\n",
    "    py.append( cstate.y )\n",
    "    \n",
    "plt.scatter( px, py, c='green' )\n",
    "#plt.show()\n",
    "    \n",
    "    \n",
    "steer = 50 * np.pi / 180\n",
    "cstate = state( 0, 0, np.pi/2, 0, 0, steer )\n",
    "px = [] \n",
    "py = []\n",
    "\n",
    "px.append( cstate.x )\n",
    "py.append( cstate.y )\n",
    "\n",
    "for i in range(150):\n",
    "    cstate = transition( cstate, steer, 0.1, Goal)\n",
    "    px.append( cstate.x )\n",
    "    py.append( cstate.y )\n",
    "    \n",
    "plt.scatter( px, py, c='red' )\n",
    "\n",
    "plt.axis('equal')\n",
    "plt.show()\n",
    "    \n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
